



#include<math.h>

#include "sl_transform.h"


//变换和坐标系都默认是右手系的。



/// <summary>
/// 使用旋转矢量和旋转角度初始化四元数
/// </summary>
/// <param name="q">四元数</param>
/// <param name="v">旋转矢量</param>
/// <param name="theta">旋转角度，弧度制的（-PI——PI）</param>
void quaternion_init(quaternion* q, vec3 v,real_t theta)
{
	theta = theta / 2;
	vec3 iv = vec3_normalize(v);
	q->w = cosf(theta);
	q->x = sinf(theta) * iv.x;
	q->y = sinf(theta) * iv.y;
	q->z = sinf(theta) * iv.z;
}

/// <summary>
/// 单位化，不改变旋转方向等信息，只把四元数伸缩成单位长度
/// </summary>
/// <param name="q">单位化的四元数</param>
void quaternion_normalize(quaternion* q)
{
	real_t d = q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z;
	d = sqrtf(d);
	if (fabs(d) < Epsilon) {
		q->w = 1;
		q->x = 0;
		q->y = 0;
		q->z = 0;
	}
	else {
		q->w = q->w / d;
		q->x = q->x / d;
		q->y = q->y / d;
		q->z = q->z / d;
	}
}

void quaternion_print(quaternion q) {
	printfs("( %f, %f ,%f, %f )", q.w, q.x, q.y, q.z);
}

/// <summary>
/// 计算四元数的叉乘。q * a
/// </summary>
/// <param name="q">四元数</param>
/// <param name="a">四元数</param>
/// <returns> q * a </returns>
quaternion quaternion_mul(quaternion q, quaternion a)
{
	quaternion b;
	b.w = q.w * a.w - q.x * a.x - q.y * a.y - q.z * a.z;
	b.x = q.w * a.x + q.x * a.w + q.y * a.z - q.z * a.y;
	b.y = q.w * a.y + q.y * a.w + q.z * a.x - q.x * a.z;
	b.z = q.w * a.z + q.z * a.w + q.x * a.y - q.y * a.x;
	return b;
}

/// <summary>
/// 计算共轭四元数
/// </summary>
/// <param name="a">四元数</param>
/// <returns>共轭四元数</returns>
quaternion quaternion_conj(quaternion a)
{
	quaternion q;
	q.w = a.w;
	q.x = -a.x;
	q.y = -a.y;
	q.z = -a.z;
	return q;
}

/// <summary>
/// 用四元数旋转三维矢量v
/// </summary>
/// <param name="q">四元数</param>
/// <param name="v">三维矢量</param>
/// <returns>旋转后的三维矢量</returns>
vec3 quaternion_rotate_usemul(quaternion q,vec3 v)
{
	vec3 v2;
	quaternion m = { 1,v.x,v.y,v.z };
	quaternion qt = quaternion_conj(q);
	quaternion qv = quaternion_mul(q, m);
	m = quaternion_mul(qv, qt);
	v2.x = m.x;
	v2.y = m.y;
	v2.z = m.z;
	return v2;
}

/// <summary>
/// 用四元数旋转三维矢量v
/// </summary>
/// <param name="q">四元数</param>
/// <param name="v">三维矢量</param>
/// <returns>旋转后的三维矢量</returns>
vec3 quaternion_rotate(quaternion q, vec3 v)
{
	vec3 v2;
	real_t q0q0 = q.w * q.w;
	real_t q0q1 = q.w * q.x;
	real_t q0q2 = q.w * q.y;
	real_t q0q3 = q.w * q.z;
	real_t q1q1 = q.x * q.x;
	real_t q1q2 = q.x * q.y;
	real_t q1q3 = q.x * q.z;
	real_t q2q2 = q.y * q.y;
	real_t q2q3 = q.y * q.z;
	real_t q3q3 = q.z * q.z;
	v2.x = (1 - 2 * (q2q2 + q3q3)) * v.x + 2 * (q1q2 - q0q3) * v.y + 2 * (q1q3 + q0q2) * v.z;
	v2.y = 2 * (q1q2 + q0q3) * v.x + (1 - 2 * (q1q1 + q3q3)) * v.y + 2 * (q2q3 - q0q1) * v.z;
	v2.z = 2 * (q1q3 - q0q2) * v.x + 2 * (q2q3 + q0q1) * v.y + (1 - 2 * (q1q1 + q2q2)) * v.z;
	return v2;
}

/// <summary>
/// 在四元数q的表示的旋转基础上，再绕X轴旋转theta角度
/// </summary>
/// <param name="q">原来的四元数</param>
/// <param name="theta">旋转角度</param>
/// <returns>旋转后的四元数</returns>
quaternion quaternion_rotateX(quaternion q, real_t theta)
{
	quaternion q2;
	real_t aw = cosf(theta / 2), ax = sinf(theta / 2), ay = 0, az = 0;
	q2.w = aw * q.w - ax * q.x - ay * q.y - az * q.z;
	q2.x = aw * q.x + ax * q.w + ay * q.z - az * q.y;
	q2.y = aw * q.y + ay * q.w + az * q.x - ax * q.z;
	q2.z = aw * q.z + az * q.w + ax * q.y - ay * q.x;
	return q2;
}

/// <summary>
/// 在四元数q的表示的旋转基础上，再绕Y轴旋转theta角度
/// </summary>
/// <param name="q">原来的四元数</param>
/// <param name="theta">旋转角度</param>
/// <returns>旋转后的四元数</returns>
quaternion quaternion_rotateY(quaternion q, real_t theta)
{
	quaternion q2;
	real_t aw = cosf(theta / 2), ay = sinf(theta / 2), ax = 0, az = 0;
	q2.w = aw * q.w - ax * q.x - ay * q.y - az * q.z;
	q2.x = aw * q.x + ax * q.w + ay * q.z - az * q.y;
	q2.y = aw * q.y + ay * q.w + az * q.x - ax * q.z;
	q2.z = aw * q.z + az * q.w + ax * q.y - ay * q.x;
	return q2;
}

/// <summary>
/// 在四元数q的表示的旋转基础上，再绕Z轴旋转theta角度
/// </summary>
/// <param name="q">原来的四元数</param>
/// <param name="theta">旋转角度</param>
/// <returns>旋转后的四元数</returns>
quaternion quaternion_rotateZ(quaternion q, real_t theta)
{
	quaternion q2;
	real_t aw = cosf(theta / 2), az = sinf(theta / 2), ay = 0, ax = 0;
	q2.w = aw * q.w - ax * q.x - ay * q.y - az * q.z;
	q2.x = aw * q.x + ax * q.w + ay * q.z - az * q.y;
	q2.y = aw * q.y + ay * q.w + az * q.x - ax * q.z;
	q2.z = aw * q.z + az * q.w + ax * q.y - ay * q.x;
	return q2;
}

/// <summary>
/// 给q0,q1两个四元数插值。t是0-1的参数，表示从q0到q1.
/// </summary>
/// <param name="q0">四元数起点</param>
/// <param name="q1">四元数终点</param>
/// <param name="t">差值的参数</param>
/// <returns>两个四元数的差值</returns>
quaternion quaternion_slerp(quaternion q0, quaternion q1, real_t t)
{
	real_t w, x, y, z;
	if (t < 0.0f) {
		w = q0.w;
		x = q0.x;
		y = q0.y;
		z = q0.z;
	}
	if (t > 1.0f) {
		w = q1.w;
		x = q1.x;
		y = q1.y;
		z = q1.z;
	}
	else {
		real_t q0w = q0.w, q0x = q0.x, q0y = q0.y, q0z = q0.z;
		real_t omega, sinOmega, cosOmega;
		cosOmega = q1.w * q0w + q1.x * q0x + q1.y * q0y + q1.z * q0z;
		//注意；四元数是【cos(theta/2)，x*sinf(theta/2)，y*sinf(theta/2)，z*sinf(theta/2)】，表示旋转2*(theta/2)
		//从旋转矩阵的角度来看，如果四元数全体取负数，得到的旋转矩阵不会改变。
		//全体取负数相当于【cos(theta/2+PI)，x*sinf(theta/2+PI)，y*sinf(theta/2+PI)，z*sinf(theta/2+PI)】，即表示旋转2*(theta/2+PI)
		//如果点乘小于0，则说明夹角大于90度，这时可以翻转一个四元数得到短的4D“弧线”。从短的弧线进行插值。
		if (cosOmega < 0.0f) {
			q0w = -q0w;
			q0x = -q0x;
			q0y = -q0y;
			q0z = -q0z;
		}
		real_t k0, k1;
		if (cosOmega > 0.99999f) {
			//夹角很小，两个四元数非常接近，使用线性插值。
			k0 = 1.0f - t;
			k1 = t;
		}
		else {
			sinOmega = sqrtf(1 - cosOmega * cosOmega);
			omega = atan2f(sinOmega, cosOmega);
			k0 = sinf((1.0f - t) * omega) / sinOmega;
			k1 = sinf(t * omega) / sinOmega;
		}
		w = k0 * q0w + k1 * q1.w;
		x = k0 * q0x + k1 * q1.x;
		y = k0 * q0y + k1 * q1.y;
		z = k0 * q0z + k1 * q1.z;
	}
	quaternion q = { w,x,y,z };
	return q;

}





/// <summary>
/// 单位化矩阵
/// </summary>
/// <param name="tm">被单位化的矩阵</param>
void trmat_identity(trmat *tm)
{
	tm->m00 = 1, tm->m01 = 0, tm->m02 = 0; tm->m03 = 0;
	tm->m10 = 0, tm->m11 = 1, tm->m12 = 0; tm->m13 = 0;
	tm->m20 = 0, tm->m21 = 0, tm->m22 = 1; tm->m23 = 0;
	tm->m30 = 0, tm->m31 = 0, tm->m32 = 0; tm->m33 = 1;
}

/// <summary>
/// 旋转平移矩阵与4*4矩阵的复制	//如果不是4*4的矩阵，就不复制。
/// tr << tm
/// </summary>
/// <param name="tr">旋转平移矩阵</param>
/// <param name="tm">4*4矩阵</param>
void trmat_copyfrom(trmat* tr,mat tm)
{
	//如果不是4*4的矩阵，就不复制。
	if ((tm.cols == 4) && (tm.rows == 4)) {
		tr->m00 = tm.data[0][0], tr->m01 = tm.data[0][1], tr->m02 = tm.data[0][2], tr->m03 = tm.data[0][3];
		tr->m10 = tm.data[1][0], tr->m11 = tm.data[1][1], tr->m12 = tm.data[1][2], tr->m13 = tm.data[1][3];
		tr->m20 = tm.data[2][0], tr->m21 = tm.data[2][1], tr->m22 = tm.data[2][2], tr->m23 = tm.data[2][3];
		tr->m30 = tm.data[3][0], tr->m31 = tm.data[3][1], tr->m32 = tm.data[3][2], tr->m33 = tm.data[3][3];
	}
}

/// <summary>
/// 旋转平移矩阵与4*4矩阵的复制	//如果不是4*4的矩阵，就不复制。
/// this >> tm
/// </summary>
/// <param name="tr">旋转平移矩阵</param>
/// <param name="tm">4*4矩阵</param>
void trmat_copyto(trmat* tr, mat tm)
{
	//如果不是4*4的矩阵，就不复制。
	if ((tm.cols == 4) && (tm.rows == 4)) {
		tm.data[0][0] = tr->m00, tm.data[0][1] = tr->m01, tm.data[0][2] = tr->m02, tm.data[0][3] = tr->m03;
		tm.data[1][0] = tr->m10, tm.data[1][1] = tr->m11, tm.data[1][2] = tr->m12, tm.data[1][3] = tr->m13;
		tm.data[2][0] = tr->m20, tm.data[2][1] = tr->m21, tm.data[2][2] = tr->m22, tm.data[2][3] = tr->m23;
		tm.data[3][0] = tr->m30, tm.data[3][1] = tr->m31, tm.data[3][2] = tr->m32, tm.data[3][3] = tr->m33;
	}
}

//用这个矩阵变换向量v0,旋转结果传递给v1。this*v0=v1

/// <summary>
/// 使用矩阵变换向量v0
/// </summary>
/// <param name="tr">旋转平移矩阵</param>
/// <param name="v0">待变换矢量</param>
/// <returns>变换后的矢量</returns>
vec3 trmat_transform(trmat tr,vec3 v0)
{
	vec3 v1;
	v1.x = tr.m00 * v0.x + tr.m10 * v0.y + tr.m20 * v0.z + tr.m30;
	v1.y = tr.m01 * v0.x + tr.m11 * v0.y + tr.m21 * v0.z + tr.m31;
	v1.z = tr.m02 * v0.x + tr.m12 * v0.y + tr.m22 * v0.z + tr.m32;
	return v1;
}

/// <summary>
/// 乘法计算 tm0 * tm1
/// </summary>
/// <param name="tm0">旋转平移矩阵</param>
/// <param name="tm1">旋转平移矩阵</param>
/// <returns> tm0 * tm1</returns>
trmat trmat_mul(trmat tm0, trmat tm1)
{
	trmat tm2;
	tm2.m00 = tm0.m00 * tm1.m00 + tm0.m01 * tm1.m10 + tm0.m02 * tm1.m20 + tm0.m03 * tm1.m30;
	tm2.m01 = tm0.m00 * tm1.m01 + tm0.m01 * tm1.m11 + tm0.m02 * tm1.m21 + tm0.m03 * tm1.m31;
	tm2.m02 = tm0.m00 * tm1.m02 + tm0.m01 * tm1.m12 + tm0.m02 * tm1.m22 + tm0.m03 * tm1.m32;
	tm2.m03 = tm0.m00 * tm1.m03 + tm0.m01 * tm1.m13 + tm0.m02 * tm1.m23 + tm0.m03 * tm1.m33;

	tm2.m10 = tm0.m10 * tm1.m00 + tm0.m11 * tm1.m10 + tm0.m12 * tm1.m20 + tm0.m13 * tm1.m30;
	tm2.m11 = tm0.m10 * tm1.m01 + tm0.m11 * tm1.m11 + tm0.m12 * tm1.m21 + tm0.m13 * tm1.m31;
	tm2.m12 = tm0.m10 * tm1.m02 + tm0.m11 * tm1.m12 + tm0.m12 * tm1.m22 + tm0.m13 * tm1.m32;
	tm2.m13 = tm0.m10 * tm1.m03 + tm0.m11 * tm1.m13 + tm0.m12 * tm1.m23 + tm0.m13 * tm1.m33;

	tm2.m20 = tm0.m20 * tm1.m00 + tm0.m21 * tm1.m10 + tm0.m22 * tm1.m20 + tm0.m23 * tm1.m30;
	tm2.m21 = tm0.m20 * tm1.m01 + tm0.m21 * tm1.m11 + tm0.m22 * tm1.m21 + tm0.m23 * tm1.m31;
	tm2.m22 = tm0.m20 * tm1.m02 + tm0.m21 * tm1.m12 + tm0.m22 * tm1.m22 + tm0.m23 * tm1.m32;
	tm2.m23 = tm0.m20 * tm1.m03 + tm0.m21 * tm1.m13 + tm0.m22 * tm1.m23 + tm0.m23 * tm1.m33;

	tm2.m30 = tm0.m30 * tm1.m00 + tm0.m31 * tm1.m10 + tm0.m32 * tm1.m20 + tm0.m33 * tm1.m30;
	tm2.m31 = tm0.m30 * tm1.m01 + tm0.m31 * tm1.m11 + tm0.m32 * tm1.m21 + tm0.m33 * tm1.m31;
	tm2.m32 = tm0.m30 * tm1.m02 + tm0.m31 * tm1.m12 + tm0.m32 * tm1.m22 + tm0.m33 * tm1.m32;
	tm2.m33 = tm0.m30 * tm1.m03 + tm0.m31 * tm1.m13 + tm0.m32 * tm1.m23 + tm0.m33 * tm1.m33;
	return tm2;
}

/// <summary>
/// 坐标系的原点位置，是默认还有一个大的背景坐标系（大地坐标系）。
/// 获得变换矩阵。根据坐标系0的原点位置，xyz方向，坐标系1的原点位置，xyz方向，计算从坐标系0变换到坐标系1的矩阵。
/// 设空间中一点p在坐标系0的坐标的p0，坐标系1的坐标是p1，两个坐标系之间的变换矩阵是this，则 p0 * this = p1.    
/// </summary>
/// <param name="position_0">  坐标系0的原点位置</param>
/// <param name="direction_0x">坐标系0的x轴方向</param>
/// <param name="direction_0y">坐标系0的y轴方向</param>
/// <param name="direction_0z">坐标系0的z轴方向</param>
/// <param name="position_1">  坐标系1的原点位置</param>
/// <param name="direction_1x">坐标系1的x轴方向</param>
/// <param name="direction_1y">坐标系1的y轴方向</param>
/// <param name="direction_1z">坐标系1的z轴方向</param>
/// <returns>从坐标系0变换到坐标系1的变换矩阵</returns>
trmat trmat_get_transformMatrix(
	vec3 position_0, vec3 direction_0x, vec3 direction_0y, vec3 direction_0z,
	vec3 position_1, vec3 direction_1x, vec3 direction_1y, vec3 direction_1z)
{
	trmat tm;
	vec3 p = vec3_sub(position_0, position_1);
	tm.m00 = vec3_dotProduct(direction_1x, direction_0x), tm.m01 = vec3_dotProduct(direction_1x, direction_0y), tm.m02 = vec3_dotProduct(direction_1x, direction_0z), tm.m03 = vec3_dotProduct(direction_1x, p);
	tm.m10 = vec3_dotProduct(direction_1y, direction_0x), tm.m11 = vec3_dotProduct(direction_1y, direction_0y), tm.m12 = vec3_dotProduct(direction_1y, direction_0z), tm.m13 = vec3_dotProduct(direction_1y, p);
	tm.m20 = vec3_dotProduct(direction_1z, direction_0x), tm.m21 = vec3_dotProduct(direction_1z, direction_0y), tm.m22 = vec3_dotProduct(direction_1z, direction_0z), tm.m23 = vec3_dotProduct(direction_1z, p);
	tm.m30 = 0, tm.m31 = 0, tm.m32 = 0, tm.m33 = 1;
	return tm;
}


/// <summary>
/// 在变换矩阵tm的基础上，增加平移变换
/// </summary>
/// <param name="tm">原矩阵</param>
/// <param name="tx">x方向平移量</param>
/// <param name="ty">y方向平移量</param>
/// <param name="tz">z方向平移量</param>
/// <returns>增加平移变换后的矩阵</returns>
trmat trmat_translate(trmat tm,real_t tx, real_t ty, real_t tz) //平移变换矩阵
{
	tm.m30 += tx;
	tm.m31 += ty;
	tm.m32 += tz;
	return tm;
}

/// <summary>
/// 在变换矩阵tm的基础上，增加比例缩放变换
/// </summary>
/// <param name="tm">原矩阵</param>
/// <param name="sx">x方向缩放系数</param>
/// <param name="sy">y方向缩放系数</param>
/// <param name="sz">z方向缩放系数</param>
/// <returns>增加平移变换后的矩阵</returns>
trmat trmat_scale(trmat tm, real_t sx, real_t sy, real_t sz)
{
	tm.m00 *= sx;
	tm.m11 *= sy;
	tm.m22 *= sz;
	return tm;
}

/// <summary>
/// 在变换矩阵tm的基础上，再绕X轴旋转theta角度
/// </summary>
/// <param name="tm">原矩阵</param>
/// <param name="theta">旋转角度</param>
/// <returns>旋转后的矩阵</returns>
trmat trmat_rotateX(trmat tm, real_t theta)
{
	real_t cos_ = cosf(theta);
	real_t sin_ = sinf(theta);
	real_t a = tm.m11, b = tm.m12, c = tm.m21, d = tm.m22;
	tm.m11 = a * cos_ - b * sin_;
	tm.m12 = a * sin_ + b * cos_;  // |a b|*| cos sin| = |m11 m12|
	tm.m21 = c * cos_ - d * sin_;  // |c d| |-sin cos|   |m21 m22|
	tm.m22 = c * sin_ + d * cos_;
	return tm;
}

/// <summary>
/// 在变换矩阵tm的基础上，再绕Y轴旋转theta角度
/// </summary>
/// <param name="tm">原矩阵</param>
/// <param name="theta">旋转角度</param>
/// <returns>旋转后的矩阵</returns>
trmat trmat_rotateY(trmat tm, real_t theta)
{
	real_t cos_ = cosf(theta);
	real_t sin_ = sinf(theta);
	real_t a = tm.m00, b = tm.m02, c = tm.m20, d = tm.m22;
	tm.m00 = a * cos_ + b * sin_;
	tm.m02 = -a * sin_ + b * cos_;  // |a b|*| cos -sin| = |m00 m02|
	tm.m20 = c * cos_ + d * sin_;  // |c d| | sin  cos|   |m20 m22|
	tm.m22 = -c * sin_ + d * cos_;
	return tm;
}

/// <summary>
/// 在变换矩阵tm的基础上，再绕Z轴旋转theta角度
/// </summary>
/// <param name="tm">原矩阵</param>
/// <param name="theta">旋转角度</param>
/// <returns>旋转后的矩阵</returns>
trmat trmat_rotateZ(trmat tm, real_t theta)
{
	real_t cos_ = cosf(theta);
	real_t sin_ = sinf(theta);
	real_t a = tm.m00, b = tm.m01, c = tm.m10, d = tm.m11;
	tm.m00 = a * cos_ - b * sin_;
	tm.m01 = a * sin_ + b * cos_;  // |a b|*| cos sin| = |m00 m01|
	tm.m10 = c * cos_ - d * sin_;  // |c d| |-sin cos|   |m10 m11|
	tm.m11 = c * sin_ + d * cos_;
	return tm;
}

void trmat_print(trmat tm) 
{
	printfs(" \r\n trmat: \r\n % f,  % f,  % f,  % f,  \r\n   % f,  % f,  % f,  % f,  \r\n   % f,  % f,  % f,  % f,  \r\n   % f,  % f,  % f,  % f,  \r\n  ",
		tm.m00, tm.m01, tm.m02, tm.m03,
		tm.m10, tm.m11, tm.m12, tm.m13,
		tm.m20, tm.m21, tm.m22, tm.m23,
		tm.m30, tm.m31, tm.m32, tm.m33
	);
}


/// <summary>
/// 使用欧拉角旋转矢量v0，按照xyz的顺序旋转
/// </summary>
/// <param name="e">欧拉角</param>
/// <param name="v0">原三维矢量</param>
/// <returns>旋转后的三维矢量</returns>
vec3 eulerAngle_rotate(eulerAngle e, vec3 v0)
{
	vec3 v1;
	//按照x,y,z轴的顺序旋转。
	real_t cx = cosf(e.angle_x);
	real_t sx = sinf(e.angle_x);
	real_t cy = cosf(e.angle_y);
	real_t sy = sinf(e.angle_y);
	real_t cz = cosf(e.angle_z);
	real_t sz = sinf(e.angle_z);

	real_t m00, m01, m02, m10, m11, m12, m20, m21, m22;  //旋转矩阵M的元素。
	m00 = cy * cz;
	m01 = cy * sz;
	m02 = -sy;
	m10 = sx * sy * cz - cx * sz;
	m11 = sx * sy * sz + cx * cz;
	m12 = sx * cy;
	m20 = sy * cx * cz + sx * sz;
	m21 = sy * cx * sz - sx * cz;
	m22 = cx * cy;
	//新矢量=旧矢量*M。  注意矩阵在乘号右边。 

	v1.x = v0.x * m00 + v0.y * m10 + v0.z * m20;
	v1.y = v0.x * m01 + v0.y * m11 + v0.z * m21;
	v1.z = v0.x * m02 + v0.y * m12 + v0.z * m22;
	return v1;
}

void eulerAngle_print(eulerAngle e) {
	printfs(" \r\n eulerAngle: \r\n % f,  % f,  % f  \r\n  ",
		e.angle_x, e.angle_y, e.angle_z);
}

/// <summary>
/// 从四元数中获取变换矩阵
/// </summary>
/// <param name="q">四元数</param>
/// <returns>变换矩阵</returns>
trmat quaternion_get_trmat(quaternion q)
{
	trmat tm;
	real_t q0q0 = q.w * q.w;
	real_t q0q1 = q.w * q.x;
	real_t q0q2 = q.w * q.y;
	real_t q0q3 = q.w * q.z;
	real_t q1q1 = q.x * q.x;
	real_t q1q2 = q.x * q.y;
	real_t q1q3 = q.x * q.z;
	real_t q2q2 = q.y * q.y;
	real_t q2q3 = q.y * q.z;
	real_t q3q3 = q.z * q.z;
	tm.m00 = (1 - 2 * (q2q2 + q3q3));
	tm.m01 = 2 * (q1q2 + q0q3);
	tm.m02 = 2 * (q1q3 - q0q2);
	tm.m03 = 0;

	tm.m10 = 2 * (q1q2 - q0q3);
	tm.m11 = (1 - 2 * (q1q1 + q3q3));
	tm.m12 = 2 * (q2q3 + q0q1);
	tm.m13 = 0;

	tm.m20 = 2 * (q1q3 + q0q2);
	tm.m21 = 2 * (q2q3 - q0q1);
	tm.m22 = (1 - 2 * (q1q1 + q2q2));
	tm.m23 = 0;

	tm.m30 = 0;
	tm.m31 = 0;
	tm.m32 = 0;
	tm.m33 = 1;
	return tm;
}

/// <summary>
/// 从四元数获取欧拉角
/// </summary>
/// <param name="q">四元数</param>
/// <returns>欧拉角</returns>
eulerAngle quaternion_get_eulerAngle(quaternion q)
{
	eulerAngle euler;
	real_t   m00, m01, m02, m10, m11, m12, m22;
	real_t q0q1 = q.w * q.x;
	real_t q0q2 = q.w * q.y;
	real_t q0q3 = q.w * q.z;
	real_t q1q1 = q.x * q.x;
	real_t q1q2 = q.x * q.y;
	real_t q1q3 = q.x * q.z;
	real_t q2q2 = q.y * q.y;
	real_t q2q3 = q.y * q.z;
	real_t q3q3 = q.z * q.z;
	m00 = (1 - 2 * (q2q2 + q3q3));
	m01 = 2 * (q1q2 + q0q3);
	m02 = 2 * (q1q3 - q0q2);
	m10 = 2 * (q1q2 - q0q3);
	m11 = (1 - 2 * (q1q1 + q3q3));
	m12 = 2 * (q2q3 + q0q1);
	m22 = (1 - 2 * (q1q1 + q2q2));
	euler.angle_y = asin(-m02);
	if (ABS(1 - ABS(m02)) > Epsilon) {  // cosf(angle_y) != 0 
		euler.angle_x = atan2f(m12, m22);
		euler.angle_z = atan2f(m01, m00);
	}
	else {
		euler.angle_x = atan2f(m10, m11);
		euler.angle_z = 0;
	}
	return euler;
}

/// <summary>
/// 从旋转矩阵获取欧拉角 （不考虑平移分量）
/// </summary>
/// <param name="tm">旋转矩阵</param>
/// <returns>欧拉角</returns>
eulerAngle trmat_get_eulerAngle(trmat tm)
{
	eulerAngle euler;
	euler.angle_y = asin(-tm.m02);
	if (ABS(1 - ABS(tm.m02)) > Epsilon) {  // cosf(angle_y) != 0 
		euler.angle_z = atan2f(tm.m01, tm.m00);
		euler.angle_x = atan2f(tm.m12, tm.m22);
	}
	else {
		euler.angle_x = atan2f(tm.m10, tm.m11);
		euler.angle_z = 0;
	}
	return euler;
}

/// <summary>
/// 从旋转矩阵获取四元数（不考虑平移分量）
/// </summary>
/// <param name="tm">旋转矩阵</param>
/// <returns>四元数</returns>
quaternion trmat_get_quaternion(trmat tm)
{
	quaternion q;
	q.w = sqrtf(tm.m00 + tm.m11 + tm.m22 + 1) / 2;
	q.x = (tm.m12 - tm.m21) / (4 * q.w);
	q.y = (tm.m20 - tm.m02) / (4 * q.w);
	q.z = (tm.m01 - tm.m10) / (4 * q.w);
	return q;
}

/// <summary>
/// 从欧拉角获取旋转矩阵
/// </summary>
/// <param name="euler">欧拉角</param>
/// <returns>旋转矩阵</returns>
trmat eulerAngle_get_trmat(eulerAngle euler)
{
	trmat tm;
	trmat_identity(&tm);
	//按照x,y,z轴的顺序旋转。
	real_t cx = cosf(euler.angle_x);
	real_t sx = sinf(euler.angle_x);
	real_t cy = cosf(euler.angle_y);
	real_t sy = sinf(euler.angle_y);
	real_t cz = cosf(euler.angle_z);
	real_t sz = sinf(euler.angle_z);

	tm.m00 = cy * cz;
	tm.m01 = cy * sz;
	tm.m02 = -sy;
	tm.m10 = sx * sy * cz - cx * sz;
	tm.m11 = sx * sy * sz + cx * cz;
	tm.m12 = sx * cy;
	tm.m20 = sy * cx * cz + sx * sz;
	tm.m21 = sy * cx * sz - sx * cz;
	tm.m22 = cx * cy;
	return tm;
}

/// <summary>
/// 从欧拉角获取四元数
/// </summary>
/// <param name="euler">欧拉角</param>
/// <returns>四元数</returns>
quaternion eulerAngle_get_quaternion(eulerAngle euler)
{
	quaternion q;
	real_t cx = cosf(euler.angle_x / 2);
	real_t sx = sinf(euler.angle_x / 2);
	real_t cy = cosf(euler.angle_y / 2);
	real_t sy = sinf(euler.angle_y / 2);
	real_t cz = cosf(euler.angle_z / 2);
	real_t sz = sinf(euler.angle_z / 2);
	q.w = cx * cy * cz + sx * sy * sz;
	q.x = sx * cy * cz - cx * sy * sz;
	q.y = cx * sy * cz + sx * cy * sz;
	q.z = cx * cy * sz - sx * sy * cz;
	return q;
}

/// <summary>
/// 从欧拉角获取四元数
/// </summary>
/// <param name="euler">欧拉角</param>
/// <returns>四元数</returns>
quaternion eulerAngle_get_quaternion_by_rotate(eulerAngle euler) //通过旋转单位四元数得到q。
{
	quaternion q = { 1,0,0,0 };
	q = quaternion_rotateX(q, euler.angle_x);
	q = quaternion_rotateY(q, euler.angle_y);
	q = quaternion_rotateZ(q, euler.angle_z);
	return q;
}

//检查四元数的变换。看用四元数乘法计算的旋转于普通计算之间的关系。
void test_Quaternion()
{
	vec3 v = { 2.35,3.69,-65.3 };
	v = vec3_normalize(v);

	real_t theta = 1.3f;
	quaternion q = { cosf(theta), sinf(theta) * v.x, sinf(theta) * v.y, sinf(theta) * v.z };

	vec3 p = { 2, 3, 4 };
	vec3 p1;
	vec3 p2;

	p1 = quaternion_rotate(q, p);
	p2 = quaternion_rotate_usemul(q, p);

	vec3_print(p1);
	vec3_print(p2);

	printfs("if two vector is same ,quaternion_rotate success \r\n ");

}

//测试欧拉角的计算
void test_EulerAngles()
{
	eulerAngle euler = { 1.2f, 0.5f, 2.3f };
	eulerAngle euler0 = { 1.2f, 0, 0 };
	eulerAngle euler1 = { 0, 0.5f, 0 };
	eulerAngle euler2 = { 0, 0, 2.3f };

	vec3 p = { 2, 3, 4 };
	vec3 p1;
	vec3 p2;
//	vec3 p3;
//	vec3 p4;

	p1 = eulerAngle_rotate(euler, p);
	vec3_print(p1);
	p2 = eulerAngle_rotate(euler0, p);
	p2 = eulerAngle_rotate(euler1, p2);
	p2 = eulerAngle_rotate(euler2, p2);
	vec3_print(p2);
	printfs("if two vector is same ,eulerAngle_rotate  success \r\n ");

}

//检查trmat的乘法运算。
void test_TransformMatrix() 
{
	mat m1,m2,m3;
	mat_allocate(&m1, 4, 4);
	mat_allocate(&m2, 4, 4);
	mat_allocate(&m3, 4, 4);

	vec v4;
	vec v41;
	vec_allocate(&v4, 4);
	vec_allocate(&v41, 4);

	trmat tm1;
	trmat tm2;
	trmat tm3;
	vec3 v3;

	vec_randomize(v4,1265654, -2, 2);

	v4.data[3] = 1;
	v3.x = v4.data[0];
	v3.y = v4.data[1];
	v3.z = v4.data[2];

	mat_randomize(m1, 156784, -3, 3);
	mat_randomize(m2, 154564, -3, 3);
	trmat_copyfrom(&tm1, m1);
	trmat_copyfrom(&tm2, m2);

	mat_mul(m1, m2, m3);
	tm3 = trmat_mul(tm1, tm2);

	mat_print(m3);
	trmat_print(tm3);
	printfs("if two mat is same ,trmat_mul  success \r\n ");

	matT_mul_vec(m3, v4, v41);
	v3 = trmat_transform(tm3, v3);

	vec3_print(v3);
	vec_print(v41);
	printfs("if two vec is same ,trmat_transform  success \r\n ");
}

//测试三种表达方式的转化。
void test_transform()
{
	test_TransformMatrix();

	{
		printfs("test  quaternion  <<==>> eulerAngle   \r\n ");
		quaternion q = { 1,0,0,0 };
		quaternion q1;
		quaternion q2;
		eulerAngle euler = { 0.7f, 1.3f, 2.3f };
		eulerAngle euler1;
//		eulerAngle euler2;
		q = quaternion_rotateX(q, euler.angle_x);
		q = quaternion_rotateY(q, euler.angle_y);
		q = quaternion_rotateZ(q, euler.angle_z);

		q1 = eulerAngle_get_quaternion(euler);
		q2 = eulerAngle_get_quaternion_by_rotate(euler);
		quaternion_print(q);
		quaternion_print(q1);
		quaternion_print(q2);


		euler1 = quaternion_get_eulerAngle(q);
		eulerAngle_print(euler);
		eulerAngle_print(euler1);

	//	std::cout << " 测试欧拉角与四元数的计算结果是否一致。  \r\n";

		vec3 v3 = { 1, 1, 1 }, v30 = { 1, 1, 1 }, v31 = { 1, 1, 1 };
		v3 = quaternion_rotate(q, v3);
		v30 = eulerAngle_rotate(euler, v30);
		vec3_print(v3);
		vec3_print(v30);

	}

	{
		printfs(" \r\n test  quaternion  <<==>> trmat   \r\n ");
		
		vec3 v = { 3.5,5.6,6.9 };
		v = vec3_normalize(v);
		real_t theta = 1.3f;
		quaternion q = { cosf(theta), sinf(theta) * v.x, sinf(theta) * v.y, sinf(theta) * v.z };
		quaternion q1;
		trmat tm1;
		trmat tm2;
		tm1 = quaternion_get_trmat(q);
		q1 = trmat_get_quaternion(tm1);
		quaternion_print(q);
		quaternion_print(q1);

		vec3 v3 = { 1, 1, 1 }, v30 = { 1, 1, 1 };
		v3 = quaternion_rotate(q, v3);
		v30 = trmat_transform(tm1, v30);

		vec3_print(v3);
		vec3_print(v30);
	}

	{
		printfs(" \r\n test  eulerAngle  <<==>> trmat   \r\n ");
		// std::cout << "\r\n 测试欧拉角与矩阵的相互转化 ,如果下面两个欧拉角相同，说明转化成功。\r\n";
		eulerAngle euler = { 0.2f, 1.4f, 2.1f };
		eulerAngle euler1;
		eulerAngle euler_0 = { 0.2f, 0, 0 };
		eulerAngle euler_1 = { 0, 1.4f, 0 };
		eulerAngle euler_2 = { 0, 0, 2.1f };
		trmat tm1;
		trmat tm2;
		trmat tm3;

		tm1 = eulerAngle_get_trmat(euler);
		euler1 = trmat_get_eulerAngle(tm1);
		eulerAngle_print(euler);
		eulerAngle_print(euler1);

		vec3 v3 = { 1, 1, 1 }, v30 = { 1, 1, 1 };
		v3 = eulerAngle_rotate(euler, v3);
		v30 = trmat_transform(tm1, v30);
		vec3_print(v3);
		vec3_print(v30);

		trmat_identity(&tm3);
		tm2 = trmat_rotateX(tm3, euler.angle_x);
		tm2 = trmat_rotateY(tm2, euler.angle_y);
		tm2 = trmat_rotateZ(tm2, euler.angle_z);
		trmat_print(tm1);
		trmat_print(tm2);

		vec3 v31 = { 1, 1, 1 }, v32 = { 1, 1, 1 };
		v31 = trmat_transform(tm1, v31);
		v32 = trmat_transform(tm2, v32);
		vec3_print(v31);
		vec3_print(v32);
	}

}

//测试反向变换
void test_fanxiangbianhuan()
{
	printfs(" \r\n test  Reverse transformation \r\n ");
	 // std::cout << "\r\n \r\n \r\n  反向转化测试  \r\n";
	quaternion q;
	quaternion q1;
	quaternion q2;
	eulerAngle euler = { 0.7f, 1.3f, 2.3f };
	eulerAngle euler1;
	eulerAngle euler2;
	trmat tm;
	trmat tm1;
	trmat tm2;
	trmat tm3;

	q = eulerAngle_get_quaternion(euler);
	tm = eulerAngle_get_trmat(euler);

	quaternion_print(q);
	trmat_print(tm);
	q1 = quaternion_conj(q); 

	euler1 = quaternion_get_eulerAngle(q1);
	tm1 = quaternion_get_trmat(q1);
	tm2 = trmat_mul(tm1, tm);
	trmat_print(tm2);
	printfs(" \r\n  it should be eye matrix \r\n ");


//	std::cout << " 测试正向变换与反向变换   \r\n";

	vec3 v3 = { 1, 1, 1 }, v30 = { 1, 1, 1 }, v31 = {1, 1, 1 }, v32 = {1, 1, 1 };
	vec3_print(v3);
	vec3_print(v30);
	vec3_print(v31);
	printfs(" Forward transformation ");
	v3 = quaternion_rotate(q, v3);
	v30 = eulerAngle_rotate(euler, v30);
	v31 = trmat_transform(tm, v31);
	vec3_print(v3);
	vec3_print(v30);
	vec3_print(v31);

	printfs(" Reverse transformation ");
	v3 = quaternion_rotate(q1, v3);
	v30 = eulerAngle_rotate(euler1, v30);
	v31 = trmat_transform(tm1, v31);
	vec3_print(v3);
	vec3_print(v30);
	vec3_print(v31);


	//Vector vec3(3);
	//vec3.randomize();
	//vec3.mul(7);

	vec3 vbias = { 2.35,-36.54,36.5 };
	tm = trmat_translate(tm, vbias.x, vbias.y, vbias.z);

	trmat_identity(&tm2);
	tm2 = trmat_translate(tm2, -vbias.x, -vbias.y, -vbias.z);
	tm3 = trmat_mul(tm2, tm1);  // tm1 是tm的逆
	trmat_print(tm);
	trmat_print(tm3);   //tm3是tm的逆
	tm2 = trmat_mul(tm, tm3);
	trmat_print(tm2);

}
